{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "ignored-gnome",
   "metadata": {},
   "source": [
    "## 导入依赖"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2766ccc2",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "ce53127e",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "average-withdrawal",
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import numpy as np\n",
    "import yaml\n",
    "# 阿凯机器人工具箱\n",
    "from kyle_robot_toolbox.transform import Transform\n",
    "from kyle_robot_toolbox.handeye_calibration import icp_solver_svd\n",
    "from kyle_robot_toolbox.robot_arm.arm5dof_uservo import Arm5DoFUServo"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cd23538d",
   "metadata": {},
   "source": [
    "## 载入九点标定的配置文件"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "160ab366",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 载入九点标定法的配置文件\n",
    "handeye_9points_config = None\n",
    "with open(\"config/handeye_calibration/handeye_9points.yaml\", 'r', encoding='utf-8') as f:\n",
    "    handeye_9points_config = yaml.load(f.read(), Loader=yaml.SafeLoader)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "straight-launch",
   "metadata": {},
   "source": [
    "## 创建机械臂"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "plastic-israeli",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 创建机械臂\n",
    "arm = Arm5DoFUServo(config_folder=\"./config\", is_init_pose=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "4fcdcd7a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 设置为阻尼模式\n",
    "arm.set_damping(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "diagnostic-produce",
   "metadata": {},
   "source": [
    "## 获取采样点"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "religious-seminar",
   "metadata": {},
   "source": [
    "将机械臂末端拖拽到9个点的位置，记录下机械臂末端的坐标值(x, y, z)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "amended-netscape",
   "metadata": {},
   "source": [
    "![](./image/p0.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "turned-estonia",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[198.4585083469577, 47.31806086993384, -48.88062408846439]"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p0 = arm.get_tool_pose()[:3]\n",
    "p0"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "buried-service",
   "metadata": {},
   "source": [
    "![](./image/p1.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "impressed-filter",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[203.21396558740506, -1.4093229238091487, -48.74860477642795]"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p1 = arm.get_tool_pose()[:3]\n",
    "p1"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fresh-bidding",
   "metadata": {},
   "source": [
    "![](./image/p2.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "black-chocolate",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[208.8678233806173, -45.98817368389381, -46.61065597984002]"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p2 = arm.get_tool_pose()[:3]\n",
    "p2"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "current-transformation",
   "metadata": {},
   "source": [
    "![](./image/p3.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "applicable-worth",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[167.31283330389329, 53.97839782179797, -47.155414780912054]"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p3 = arm.get_tool_pose()[:3]\n",
    "p3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "visible-hamburg",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[174.96088021165292, 3.33715239062646, -47.10764092830675]"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p4 = arm.get_tool_pose()[:3]\n",
    "p4"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "naughty-contribution",
   "metadata": {},
   "source": [
    "![](./image/p5.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "hourly-enhancement",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[172.60225838357272, -43.69187312141745, -46.9208920996052]"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p5 = arm.get_tool_pose()[:3]\n",
    "p5"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "individual-pulse",
   "metadata": {},
   "source": [
    "![](./image/p6.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "younger-florence",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[130.02363058829326, 49.54939006709329, -45.42179758995064]"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p6 = arm.get_tool_pose()[:3]\n",
    "p6"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "color-conditions",
   "metadata": {},
   "source": [
    "![](./image/p7.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "vertical-packet",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[130.55660067670573, 9.29618678372697, -45.51926620477738]"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p7 = arm.get_tool_pose()[:3]\n",
    "p7"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "characteristic-marshall",
   "metadata": {},
   "source": [
    "![](./image/p8.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "broad-shopper",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[124.62772578563606, -42.84812796665811, -45.705479306754285]"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p8 = arm.get_tool_pose()[:3]\n",
    "p8"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "tropical-setup",
   "metadata": {},
   "source": [
    "## 数据保存"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "attended-hacker",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[198.459,  47.318, -48.881],\n",
       "       [203.214,  -1.409, -48.749],\n",
       "       [208.868, -45.988, -46.611],\n",
       "       [167.313,  53.978, -47.155],\n",
       "       [174.961,   3.337, -47.108],\n",
       "       [172.602, -43.692, -46.921],\n",
       "       [130.024,  49.549, -45.422],\n",
       "       [130.557,   9.296, -45.519],\n",
       "       [124.628, -42.848, -45.705]], dtype=float32)"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "ws_9p_at_arm = np.float32([p0, p1, p2, p3, p4, p5, p6, p7, p8])\n",
    "ws_9p_at_arm"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "given-brake",
   "metadata": {},
   "source": [
    "将数据存放到csv文件中"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "talented-bracelet",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.savetxt('config/handeye_calibration/arm9points.txt', ws_9p_at_arm, delimiter=',', fmt='%.1f')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "01be90e6",
   "metadata": {},
   "source": [
    "## Base标定"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "id": "0f11f4d5",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "9点在机械臂坐标系的坐标: \n",
      "[[198.5    47.3   -46.889]\n",
      " [203.2    -1.4   -46.889]\n",
      " [208.9   -46.    -46.889]\n",
      " [167.3    54.    -46.889]\n",
      " [175.      3.3   -46.889]\n",
      " [172.6   -43.7   -46.889]\n",
      " [130.     49.5   -46.889]\n",
      " [130.6     9.3   -46.889]\n",
      " [124.6   -42.8   -46.889]]\n",
      "9点在工作台坐标系的坐标: \n",
      "[[ 43.5  62.5   0. ]\n",
      " [ 43.5   0.    0. ]\n",
      " [ 43.5 -62.5   0. ]\n",
      " [  0.   62.5   0. ]\n",
      " [  0.    0.    0. ]\n",
      " [  0.  -62.5   0. ]\n",
      " [-43.5  62.5   0. ]\n",
      " [-43.5   0.    0. ]\n",
      " [-43.5 -62.5   0. ]]\n",
      "T_arm2ws: \n",
      "[[  1.      0.002   0.    167.856]\n",
      " [ -0.002   1.      0.      3.278]\n",
      " [  0.      0.      1.    -46.889]\n",
      " [  0.      0.      0.      1.   ]]\n"
     ]
    }
   ],
   "source": [
    "# 设置Numpy的打印选项\n",
    "# 精确位数3，不启用科学计数法\n",
    "np.set_printoptions(precision=3, suppress=True)\n",
    "\n",
    "# 载入九点标定法的配置文件\n",
    "handeye_9points_config = None\n",
    "with open(\"config/handeye_calibration/handeye_9points.yaml\", 'r', encoding='utf-8') as f:\n",
    "\thandeye_9points_config = yaml.load(f.read(), Loader=yaml.SafeLoader)\n",
    "\n",
    "\n",
    "# 载入机械臂末端达到九个点\n",
    "# 载入九点在2D图像中的坐标\n",
    "arm_9points = np.loadtxt(\"config/handeye_calibration/arm9points.txt\", delimiter=',')\n",
    "# 主要注意的是，对于关节臂构型的机械臂\n",
    "# 有的在Z轴上的误差很大，因此需要将\n",
    "# arm_9points里面Z轴坐标替换为Z轴的均值。\n",
    "z_mean = np.mean(arm_9points[:, 2])\n",
    "\n",
    "# z轴误差纠正拟合\n",
    "# - 转化为r跟z的形式\n",
    "r = np.sqrt(arm_9points[:, 0]**2 + arm_9points[:, 1]**2)\n",
    "z_error =  arm_9points[:, 2] - z_mean\n",
    "z_error_polyfit = np.polyfit(r, z_error, 1)\n",
    "np.savetxt(\"config/handeye_calibration/z_error_polyfit.txt\", z_error_polyfit, fmt='%.4f', delimiter=\",\")\n",
    "arm_9points[:, 2] = z_mean\n",
    "print(f\"9点在机械臂坐标系的坐标: \\n{arm_9points}\")\n",
    "\n",
    "# 9点在工作台坐标系的坐标\n",
    "# ws_9points_df = pd.read_excel(\"config/九点坐标-工作台坐标系.xlsx\")\n",
    "# ws_9points = np.float64(ws_9points_df.to_numpy())[:, 1:]\n",
    "w = handeye_9points_config[\"board_width\"]\n",
    "h = handeye_9points_config[\"board_height\"]\n",
    "# P0点的坐标\n",
    "x0 = 0.5 * h\n",
    "y0 = 0.5 * w\n",
    "ws_9points = np.float64([\n",
    "\t[x0, y0, 0], \t#P0\n",
    "\t[x0, 0, 0], \t#P1\n",
    "\t[x0, -y0, 0], \t#P2\n",
    " \t[0, y0, 0], \t#P3\n",
    "\t[0, 0, 0], \t\t#P4\n",
    "\t[0, -y0, 0], \t#P5\n",
    "\t[-x0, y0, 0], \t#P6\n",
    "\t[-x0, 0, 0], \t#P7\n",
    "\t[-x0, -y0, 0], \t#P8\n",
    "])\n",
    "print(f\"9点在工作台坐标系的坐标: \\n{ws_9points}\")\n",
    "\n",
    "# 求解机械臂基坐标系到工作台坐标系的变换\n",
    "R, t = icp_solver_svd(arm_9points, ws_9points)\n",
    "T_arm2ws = np.eye(4)\n",
    "T_arm2ws[:3, :3] = R\n",
    "T_arm2ws[:3, 3] = t.reshape(-1)\n",
    "print(\"T_arm2ws: \\n{}\".format(T_arm2ws))\n",
    "np.savetxt(\"config/handeye_calibration/T_arm2ws.txt\", T_arm2ws, fmt='%.3f', delimiter=\",\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
